#include "ber.h"
#include "ansi_interface.h"

#define __BER__

#define NULL ((void *)0)

int ber_word16_encode(unsigned short d, bercode_t *ber)
{
	unsigned char *p = ber->buf;

	if (d < 128) {
		*p = d;
		ber->len = 1;
	} else if (d < 256) {
		*p++ = 0x81;
		*p = d;
		ber->len = 2;
	} else {
		*p++ = 0x82;
		*p++ = d >> 8;
		*p = d;
		ber->len = 3;
	}

	return 0;
}

int ber_word16_decode(unsigned short *d, bercode_t *ber)
{
	switch (ber->len) {
	case 1:
		*d = ber->buf[0];
		break;
	case 2:
		*d = ber->buf[1];
		break;
	case 3:
		*d = ber->buf[1] << 8 | ber->buf[2];
		break;
	default:
		break;
	}

	return 0;
}


static uint8 key[64] = {
0,0,1,1,0,0,0,1,0,0,1,1,0,0,0,1,0,0,1,1,0,0,0,1,0,0,1,1,0,0,0,1,
0,0,1,1,0,0,0,1,0,0,1,1,0,0,0,1,0,0,1,0,0,0,0,0,0,0,1,0,0,0,0,0
};
static uint8 data[64] = {
0,1,1,1,0,0,1,0,0,1,1,0,1,0,1,0,1,1,0,1,0,0,0,1,0,0,1,0,1,0,0,1,
0,0,1,1,1,0,0,0,0,0,1,1,0,1,1,0,1,1,0,0,1,0,0,1,1,1,1,0,0,0,1,1
};
static uint8 perm1[56] = {
57, 49, 41, 33, 25, 17, 9, 1, 58, 50, 42, 34, 26, 18,
10, 2, 59, 51, 43, 35, 27, 19, 11, 3, 60, 52, 44, 36,
63, 55, 47, 39, 31, 23, 15, 7, 62, 54, 46, 38, 30, 22,
14, 6, 61, 53, 45, 37, 29, 21, 13, 5, 28, 20, 12, 4
};
static uint8 perm2[56] = {
2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15,
16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 1,
30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43,
44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 29
};
static uint8 perm3[48] = {
14, 17, 11, 24, 1, 5, 3, 28, 15, 6, 21, 10, 23, 19, 12, 4,
26, 8, 16, 7, 27, 20, 13, 2, 41, 52, 31, 37, 47, 55, 30, 40,
51, 45, 33, 48, 44, 49, 39, 56, 34, 53, 46, 42, 50, 36, 29, 32
};
static uint8 perm4[64] = {
58, 50, 42, 34, 26, 18, 10, 2, 60, 52, 44, 36, 28, 20, 12, 4,
62, 54, 46, 38, 30, 22, 14, 6, 64, 56, 48, 40, 32, 24, 16, 8,
57, 49, 41, 33, 25, 17, 9, 1, 59, 51, 43, 35, 27, 19, 11, 3,
61, 53, 45, 37, 29, 21, 13, 5, 63, 55, 47, 39, 31, 23, 15, 7,
};
static uint8 perm5[48] = {
32, 1, 2, 3, 4, 5, 4, 5, 6, 7, 8, 9, 8, 9, 10, 11,
12, 13, 12, 13, 14, 15, 16, 17, 16, 17, 18, 19, 20, 21, 20, 21,
22, 23, 24, 25, 24, 25, 26, 27, 28, 29, 28, 29, 30, 31, 32, 1,
};
static uint8 perm6[32] = {
16, 7, 20, 21, 29, 12, 28, 17, 1, 15, 23, 26, 5, 18, 31, 10,
2, 8, 24, 14, 32, 27, 3, 9, 19, 13, 30, 6, 22, 11, 4, 25,
};
static uint8 perm7[64] = {
40, 8, 48, 16, 56, 24, 64, 32, 39, 7, 47, 15, 55, 23, 63, 31,
38, 6, 46, 14, 54, 22, 62, 30, 37, 5, 45, 13, 53, 21, 61, 29,
36, 4, 44, 12, 52, 20, 60, 28, 35, 3, 43, 11, 51, 19, 59, 27,
34, 2, 42, 10, 50, 18, 58, 26, 33, 1, 41, 9, 49, 17, 57, 25,
};
static uint8 sboxes[8][64] = {
{
14,4,13,1,2,15,11,8,3,10,6,12,5,9,0,7,0,15,7,4,14,2,13,1,10,6,12,11,9,5,3,8,
4,1,14,8,13,6,2,11,15,12,9,7,3,10,5,0,15,12,8,2,4,9,1,7,5,11,3,14,10,0,6,13,
},{
15,1,8,14,6,11,3,4,9,7,2,13,12,0,5,10,3,13,4,7,15,2,8,14,12,0,1,10,6,9,11,5,
0,14,7,11,10,4,13,1,5,8,12,6,9,3,2,15,13,8,10,1,3,15,4,2,11,6,7,12,0,5,14,9,
},{
10,0,9,14,6,3,15,5,1,13,12,7,11,4,2,8,13,7,0,9,3,4,6,10,2,8,5,14,12,11,15,1,
13,6,4,9,8,15,3,0,11,1,2,12,5,10,14,7,1,10,13,0,6,9,8,7,4,15,14,3,11,5,2,12,
},{
7,13,14,3,0,6,9,10,1,2,8,5,11,12,4,15,13,8,11,5,6,15,0,3,4,7,2,12,1,10,14,9,
10,6,9,0,12,11,7,13,15,1,3,14,5,2,8,4,3,15,0,6,10,1,13,8,9,4,5,11,12,7,2,14,
},{
2,12,4,1,7,10,11,6,8,5,3,15,13,0,14,9,14,11,2,12,4,7,13,1,5,0,15,10,3,9,8,6,
4,2,1,11,10,13,7,8,15,9,12,5,6,3,0,14,11,8,12,7,1,14,2,13,6,15,0,9,10,4,5,3,
},{
12,1,10,15,9,2,6,8,0,13,3,4,14,7,5,11,10,15,4,2,7,12,9,5,6,1,13,14,0,11,3,8,
9,14,15,5,2,8,12,3,7,0,4,10,1,13,11,6,4,3,2,12,9,5,15,10,11,14,1,7,6,0,8,13,
},{
4,11,2,14,15,0,8,13,3,12,9,7,5,10,6,1,13,0,11,7,4,9,1,10,14,3,5,12,2,15,8,6,
1,4,11,13,12,3,7,14,10,15,6,8,0,5,9,2,6,11,13,8,1,4,10,7,9,5,0,15,14,2,3,12,
},{
13,2,8,4,6,15,11,1,10,9,3,14,5,0,12,7,1,15,13,8,10,3,7,4,12,5,6,11,0,14,9,2,
7,11,4,1,9,12,14,2,0,6,10,13,15,3,5,8,2,1,14,7,4,10,8,13,15,12,9,0,3,5,6,11,
}};

static uint8 keys[16][48];

void Permutation(uint8 *dst, uint8 *src, uint8 lgn, uint8 *perm_table)
{
    uint8 tmp[64];

    if (src == NULL)
    {
        src = tmp;
        memcpy(src, dst, 64);
    }
    for (; lgn > 0; lgn--, dst++, perm_table++)
        *dst = src[*perm_table - 1];
}

void Xor(uint8 *dst, uint8 *src, uint8 lgn)
{
	for (; lgn > 0; lgn--, dst++, src++)
	*dst ^= *src;
}

void Copy(uint8 *dst, uint8 *src, uint8 lgn)
{
    for (; lgn > 0; lgn--, dst++, src++)
        *dst = *src;
}

void SBoxes(uint8 *dst, uint8 *src, uint8 *sbox)
{
	int i;
	i = src[4];
	i |= src[3] << 1;
	i |= src[2] << 2;
	i |= src[1] << 3;
	i |= src[5] << 4;
	i |= src[0] << 5;
	i = sbox[i];
	dst[3] = i & 1;
	dst[2] = i >> 1 & 1;
	dst[1] = i >> 2 & 1;
	dst[0] = i >> 3 & 1;
}

void des(uint8 *_key, uint8 *_data, int _encrypt)
{
	uint8 key[64], data[64], right[48];
	int i, j;
	Permutation(key, _key, 56, perm1);
	for (i = 1; i <= 16; i++)
	{
		Permutation(key, NULL, 56, perm2);
		if (i != 1 && i != 2 && i != 9 && i != 16)
		Permutation(key, NULL, 56, perm2);
		Permutation(keys[_encrypt ? i - 1 : 16 - i], key, 48, perm3);
	}
	Permutation(data, _data, 64, perm4);
	for (i = 1; i <= 16; i++)
	{
		Permutation(right, data + 32, 48, perm5);
		Xor(right, keys[i - 1], 48);
		for (j = 0; j < 8; j++)
		SBoxes(right + 4 * j, right + 6 * j, sboxes[j]);
		Permutation(right, NULL, 32, perm6);
		Xor(right, data, 32);
		Copy(data, data + 32, 32);
		Copy(data + 32, right, 32);
	}
	Copy(_data, data + 32, 32);
	Copy(_data + 32, data, 32);
	Permutation(_data, NULL, 64, perm7);
}

static void transDataTblToBitTbl(unsigned char *pSrc, unsigned char *pDst)
{
	int i, j, k = 63;
	unsigned char val = 0;

	for (i = 7; i >=0; i--)
	{
		val = pSrc[i];

		for (j = 0; j < 8; j++, k--)
		{
			pDst[k] = val & 1;
			val = val >> 1;
		}
	}
}

static void tranBitTblToDataTbl(unsigned char *pSrc, unsigned char *pDst)
{
	int i, j, k = 63;
	unsigned char val;

	for (i = 7; i >= 0; i--)
	{
		val = 0;
		for (j = 0; j < 8; j++, k--)
		{
			val |= (pSrc[k] << j);
		}
		pDst[i] =  val;
	}

}

void des_calc(unsigned char *psrc, unsigned char *pdst)
{
	transDataTblToBitTbl(psrc, data);

	des(key, data, 1);

	tranBitTblToDataTbl(data, pdst);
}
